# -*- coding: utf-8 -*-
"""
Created on Wed Apr  5 13:11:11 2023

@author: ZNZZ
"""
# Make sure to have the server side running in CoppeliaSim: 
# in a child script of a CoppeliaSim scene, add following command
# to be executed just once, at simulation start:
#
# simRemoteApi.start(19999)
#
# then start simulation, and run this program.
#
# IMPORTANT: for each successful call to simxStart, there
# should be a corresponding call to simxFinish at the end!


'''
机械臂操作、角度计算相关的函数
编写：ZNZZ  执念执战 WCC
QQ: 572314251
blogs: https://home.cnblogs.com/u/zhinianzhizhan/

'''



try:
    import sim
except:
    print ('--------------------------------------------------------------')
    print ('"sim.py" could not be imported. This means very probably that')
    print ('either "sim.py" or the remoteApi library could not be found.')
    print ('Make sure both are in the same folder as this file,')
    print ('or appropriately adjust the file "sim.py"')
    print ('--------------------------------------------------------------')
    print ('')

import time
import math 

class ArmSizeBasicClass():
    def __init__(self):
        self.ArmID = 0
        self.ArmLength = 0
        self.ArmAngle = 0
        


class ArmSectionPointBasicClass():
    def __init__(self):
        self.PointNum = 0
        self.Point1 = [0,0]
        self.Point1Angle = 0
        self.Point2 = [0,0]
        self.Point2Angle = 0


class MyArmBasicClass():
    
    def __init__(self):
        print('MyArmTest Program started')
        self.clientID = 0
        self.Handle={  #字典，用于保存各个模块的句柄，方便拓展和查找；key值要和CoppeliaSim中的模块命名一致
            "ShapeBase":0,
            "ShapArm1":0,
            "ShapArm2":0,
            "ShapArm3":0,
            "BaseJoint":0,
            "ArmJoint1":0,
            "ArmJoint2":0,
            "ArmJoint3":0,
            }
        self.HandleOrder=(   #元组有固定的顺序，所以用元组作为顺序记录，要和上面的Handle的一致
            "ShapeBase",
            "ShapArm1",
            "ShapArm2",
            "ShapArm3",
            "BaseJoint",
            "ArmJoint1",
            "ArmJoint2",  
            "ArmJoint3", 
            )
        
        self.Arm1Size = ArmSizeBasicClass()
        self.Arm1Size.ArmID = 1
        self.Arm1Size.ArmLength = 30
        self.Arm1Size.ArmAngle = 0
        self.Arm2Size = ArmSizeBasicClass()
        self.Arm2Size.ArmID = 2
        self.Arm2Size.ArmLength = 20
        self.Arm2Size.ArmAngle = 0        
        
        
        
        self.ArmSectionPoint = ArmSectionPointBasicClass()
        
    def ConnectedStart(self):
        '''
        连接初始化。
        需要首先在 CoppeliaSimEdu 中点击运行，再运行本python程序，才能正确连接。
        返回-1表示连接失败

        Returns
        -------
        TYPE
            DESCRIPTION.

        '''
        sim.simxFinish(-1) # just in case, close all opened connections
        self.clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim
        if self.clientID!=-1:
            print ('Connected to remote API server')
            return 0
        else:
            print ('Connected faile,please check!')
            return -1
        
    def GetArmHandle(self):
        '''
        获取各个模块的句柄，用于操控
        有些模块的句柄获取不到
        
        '''
        for i in  self.HandleOrder:
            self.Handle[i]  = sim.simxGetObjectHandle(self.clientID, i, sim.simx_opmode_blocking) #获取句柄,返回两个值，一个是ret，用于判断是否获取成功，一个是obj，表示句柄号，两个值以元组的方式存到Handle中
            print(self.Handle[i][0])
            if self.Handle[i][0] != sim.simx_return_ok:
                print("Get "+i+" Handle Error!!") 
            else:
                print("Get "+i+" Handle OK!!") 
                   #实测可以看到，只获取了 底座和三个关节的句柄，所以能操控底座的位置，能操控三个关节的角度，但是不能操作三个臂
                   
    def GetShapeBasePosition(self):
        '''
        获取底座的位置
        
        '''          
        if self.clientID!=-1:
            if self.Handle[self.HandleOrder[0]][0] != sim.simx_return_ok:
                print("Get "+self.HandleOrder[0]+" Handle Error!!") 
            else:
                ret, arr = sim.simxGetObjectPosition(self.clientID, self.Handle[self.HandleOrder[0]][1], -1, sim.simx_opmode_blocking)
                print(ret,arr)
                return ret, arr
        else:    
            print("Something Error!!")
            
                
    def SetShapeBasePosition(self,X,Y,Z):
        '''
        设置底座的位置

        Parameters
        ----------
        (X,Y,Z) : TYPE
            DESCRIPTION:目标坐标（世界坐标系）

        Returns
        -------
        None.

        '''
        if self.clientID!=-1:
            if self.Handle[self.HandleOrder[0]][0] != sim.simx_return_ok:
                print("Get "+self.HandleOrder[0]+" Handle Error!!") 
            else:
                sim.simxSetObjectPosition(self.clientID, self.Handle[self.HandleOrder[0]][1],-1,(X,Y,Z), sim.simx_opmode_blocking)
                print("Set ShapeBase Pos to X:"+str(X)+" Y:"+str(Y)+"  Z:"+str(Z))
        
        else:    
            print("Something Error in SetShapeBasePosition!!")
        
        
        
    
    def GetJointAngle(self,num):
        '''
        获取旋转关节的角度

        Parameters
        ----------
        num : TYPE：控制哪个joint关节
            DESCRIPTION：可以输入 0 1 2  分分别表示 ShapeBase ArmJoint1 ArmJoint2 
                        也可以直接输入字符串 ShapeBase ArmJoint1 ArmJoint2 

        Returns
        -------
        None.

        '''
        if self.clientID!=-1:
            if self.Handle[self.HandleOrder[0]][0] != sim.simx_return_ok:
                print("Get "+self.HandleOrder[0]+" Handle Error!!") 
            else:
                if str(type(num)) == "<class 'int'>":  #先判断输入的num类型
                    if num==1:
                        targetObj_Revolute_joint = "ArmJoint1"
                    elif num == 2:
                        targetObj_Revolute_joint = "ArmJoint2"
                    elif num == 0:
                        targetObj_Revolute_joint = "ShapeBase"
                    else:
                        print("Joint num Error !!")
                        return 
                elif str(type(num)) == "<class 'str'>":
                    targetObj_Revolute_joint = num
                    
                else:
                    print("Joint num type Erroe,Pleace give 1 or 2 or stringName")
                    
                position = sim.simxGetJointPosition(self.clientID, self.Handle[targetObj_Revolute_joint][1], sim.simx_opmode_blocking)
                
                print("Joint "+targetObj_Revolute_joint + " Angle is "+str(position))
                return position
                
        else:    
            print("Something Error in GetJointAngle!!")
        
        
        
    
        
        
    
    def SetJointAngle(self,num,angle):
        '''
        设置关节角度/位置
        对于旋转关节，是设置角度值，内部需要转换为弧度；对于移动关节，我还没搞明白其单位，推测是米，所以mm需要除以1000

        Parameters
        ----------
        num : TYPE：控制哪个joint关节
            DESCRIPTION：可以输入 0 1 2  分分别表示 ShapeBase ArmJoint1 ArmJoint2 
                        也可以直接输入字符串 ShapeBase ArmJoint1 ArmJoint2 
        angle : TYPE  对于  ArmJoint1 ArmJoint2 ，为旋转得到角度值，对于ShapeBase，就是拉伸，暂时没注意具体拉伸多少
            DESCRIPTION.

        Returns
        -------
        None.

        '''
        if self.clientID!=-1:
            if self.Handle[self.HandleOrder[0]][0] != sim.simx_return_ok:
                print("Get "+self.HandleOrder[0]+" Handle Error!!") 
            else:
                if str(type(num)) == "<class 'int'>":  #先判断输入的num类型
                    if num==1:
                        targetObj_Revolute_joint = "ArmJoint1"
                    elif num == 2:
                        targetObj_Revolute_joint = "ArmJoint2"
                    elif num == 0:
                        targetObj_Revolute_joint = "ShapeBase"                        
                        
                    else:
                        print("Joint num Error !!")
                        return -1
                elif str(type(num)) == "<class 'str'>":
                    targetObj_Revolute_joint = num
                    
                else:
                    print("Joint num type Erroe,Pleace give 1 or 2 or stringName")
                    return -1
                    
                    
                if  targetObj_Revolute_joint ==    "ArmJoint1" or  targetObj_Revolute_joint ==    "ArmJoint2":
                    setangle = angle*math.pi/180#角度要转为弧度, 2π=360度
                else:
                    #ShapeBase 关节不是角度，是运行，目前还没弄清数值与实际运动的关系
                    setangle = angle
                
                sim.simxSetJointPosition(self.clientID, self.Handle[targetObj_Revolute_joint][1], setangle, sim.simx_opmode_blocking) 
                print("Set " + targetObj_Revolute_joint + "Angle to "+str(angle))

               
        else:    
            print("Something Error in GetJointAngle!!")    
    
        
    
    
    

        
    def CalPointOfSection(self,x1,y1,r1,x2,y2,r2):
        '''
        根据臂两端坐标，计算圆的交点坐标
        '''
        D12 = math.sqrt((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1))
        Def = r1 + r2 - D12
        Deo = Def / 2
        Dae = r1 - Def
        Dao = Dae + Deo
        if Dao / r1 >= 1 or  Dao / r1 <=-1:
            th2 = 0
        else:
            th2 = math.acos(Dao / r1)
        if x2 - x1 == 0 :
            th1 = 0
        else:
            th1 = math.atan((y2 - y1)/(x2 - x1))
        X = x1 + r1*math.cos(th1 + th2)
        Y = y1 + r1*math.sin(th1 + th2)
        
        return X,Y
        

    
    def CalAngleOfPoint1(self,x1,y1,x2,y2,r):
        '''
        根据两个坐标计算角度，用于实现电机旋转
        x1,y1,x2,y2：角度所在直线的两个端点坐标，即某一个臂的两端端点坐标
        r:臂长
        
        '''

        #return 90 + math.atan( D1/D2)/math.pi * 180
        
        
        angle = math.asin(abs(x2-x1)/r) /math.pi*180
        print("CalAbgleOfPoint1 angle:",angle)
        
        if x2-x1>0:
            return angle
        else:
            return -angle 
            
    def CalAngleBetweenTwoPoint(self,x1,y1,x2,y2,r):
        '''
        两交点点连线的中轴线法求角度
        本角度求得是两个交点分别于原点连线形成的夹角角度
        方法是使用两交点连线，做过圆心的中垂线，根据形成的直角三角形的关系求角度
        
        x1,y1,x2,y2:分别是两个交点的坐标
        r:第一臂的长度

        Parameters
        ----------
        x1 : TYPE
            DESCRIPTION.
        y1 : TYPE
            DESCRIPTION.
        x2 : TYPE
            DESCRIPTION.
        y2 : TYPE
            DESCRIPTION.
        r : TYPE
            DESCRIPTION.

        Returns
        -------
        angle : TYPE
            DESCRIPTION.

        '''
        #两点连线的中轴线法求角度
        #具体方法：圆上任意两点连线的中垂线一定过圆心，其中一点与圆心与中垂点三点组成的三角形一定是垂直三角形
        #设 ∠A为两点夹角，L为两点连线长度,半径为R，则sin(A/2)=(L/2)/R  可得 A/2=asin(L/2/R) = asin(sqrt((x2-x1)^2 + (y2-y1)^2)/2/R)
        #得出的值需要换算为角度，即： A/3.1415*180
        
        angle1 = (x2 - x1)
        angle2 = (y2 - y1)
        data = math.asin(math.sqrt(angle1*angle1 + angle2 * angle2)/2/r)
        angle= data/3.1415926*180*2
        print("CalAbgleBetweenTwoPoint Angle :angle1:%f,angle2:%f,angle:%f"%(angle1,angle2,angle))
        return angle
    
    def CalAngleOfArm2(self,xp,yp,r1,xg,yg,r2,A1):
        '''
        

        Parameters
        ----------
        xp,yp:两臂第一个交点的坐标（如果要用第二个焦点，自行更改）
        xg,yg：鼠标点击点坐标，即臂末端坐标
        r1,r2：两个臂的长短坐标
        A1：两个交点与圆心夹角的角度的1/2，即第一臂与末端和原点连线的夹角，计算第一臂的运动角度时会计算出来
        
        返回值是两个臂之间的夹角角度
        
        Returns
        -------
        None.

        '''
        A1 = A1/180 * math.pi  #转为弧度
        L1 = r1*math.cos(A1)
        L2 = math.sqrt(xg*xg+yg*yg) - L1
        angle = math.asin(L2/r2) *180/math.pi #转为角度
        return 90-A1 + angle
        
        
        
        
    
    def CalPointOfSection1(self,x1,y1,r1,x2,y2,r2):
        '''
        一种方法实现求圆的交点坐标。
        注意，(x1,x),(y1,y2)是两个臂非交点的坐标点，(x1,y1)默认为(0,0,),(x2,y2)是机械臂的末端端点位置，也是鼠标点击的坐标位置
        r1,r2则是两个臂的长度，默认为r1 = 30cm，r2=20cm.后续可能会使用0.01mm的长度单位
        Parameters
        ----------
        x1 : TYPE
            DESCRIPTION.
        y1 : TYPE
            DESCRIPTION.
        r1 : TYPE
            DESCRIPTION.
        x2 : TYPE
            DESCRIPTION.
        y2 : TYPE
            DESCRIPTION.
        r2 : TYPE
            DESCRIPTION.

        Returns
        -------
        TYPE
            DESCRIPTION.

        '''
        #https://blog.csdn.net/qq_18509807/article/details/84950132
        #使用了里面的方法一
        
        dL = math.sqrt((x2-x1)*(x2-x1) +(y2 - y1) * (y2-y1))  #两点之间的长度
        print("dL:",dL)
        
        
        if dL == 0:
            print("dL = 0")
            self.ArmSectionPoint.PointNum = 0
        elif dL > (r1+r2):  #大于两臂长
            print("大于两臂长")
            self.ArmSectionPoint.PointNum = 0
        elif dL< abs(r1-r2): #小于两臂差
            print("小于两臂差")
            self.ArmSectionPoint.PointNum = 0
        elif dL == r1+r2: #等于两臂长
            print("等于两臂长")
            self.ArmSectionPoint.PointNum = 1
            AngleXL = math.asin(dL/(x2-x1))  #求出末端点与原点连线相对于y坐标轴的夹角   
            self.ArmSectionPoint.Point1[0]= round(x1 + r1 * math.sin(AngleXL),4)
            self.ArmSectionPoint.Point1[1]= round(y1 + r1 * math.cos(AngleXL),4)
            
            
        elif dL == abs(r1-r2): #等于两臂差
            print("等于两臂差")
            self.ArmSectionPoint.PointNum = 1
            AngleXL = math.asin(dL/(x2-x1))  #求出末端点与原点连线相对于y坐标轴的夹角   
            self.ArmSectionPoint.Point1[0]= round(x1 + r1 * math.sin(AngleXL),4)
            self.ArmSectionPoint.Point1[1]= round(y1 + r1 * math.cos(AngleXL),4)
            
        else:  #两个交点
            print("两个交点")
            self.ArmSectionPoint.PointNum = 2
            
            
            AE = (r1*r1 - r2 * r2 + dL*dL)/(2*dL)
            PEx = x1 + AE /dL * (x2 - x1) 
            PEy = y1 + AE /dL * (y2 - y1) 
            PointE = [PEx,PEy]
            if x2-x1 != 0:
                K1 = (y2-y1)/(x2-x1)
                if K1 != 0:
                    K2 = -1/K1
                else:
                    K2 = -1/0.0001
            else:
                K1 = -1
                K2 = -1/K1
            CE = math.sqrt(r1*r1-(PEx-x1)*(PEx-x1)-(PEy-y1)*(PEy-y1))
            EF = CE/math.sqrt(1+K2*K2)
            self.ArmSectionPoint.Point1[0]=  round(PEx - EF,4)
            self.ArmSectionPoint.Point1[1]=  round(PEy + K2*(self.ArmSectionPoint.Point1[0] - PEx),4)
            self.ArmSectionPoint.Point2[0]=  round(PEx + EF,4)
            self.ArmSectionPoint.Point2[1]=  round(PEy + K2*(self.ArmSectionPoint.Point2[0] - PEx),4)
        
        return self.ArmSectionPoint
        
        

    def CalPointOfSection2(self,x1,y1,r1,x2,y2,r2):
        '''
        另一种方式实现求两个圆的交点
        注意，(x1,x),(y1,y2)是两个臂非交点的坐标点，其中(x1,y1)默认为(0,0,),(x2,y2)是机械臂的末端端点位置，也是鼠标点击的坐标位置
        r1,r2则是两个臂的长度，默认为r1 = 30cm，r2=20cm.后续可能会使用0.01mm的长度单位
        '''
        x = x1
        y = y1
        R = r1
        a = x2
        b = y2
        S = r2
        d = math.sqrt((abs(a - x)) ** 2 + (abs(b - y)) ** 2) #求两点间距离
        if d > (R + S) or d < (abs(R - S)): #判断范围是偶发合理
            #print("Two circles have no intersection")
            self.ArmSectionPoint.PointNum = -1
            return self.ArmSectionPoint
        elif d == 0:
            #print("Two circles have same center!")
            self.ArmSectionPoint.PointNum = -2
            return self.ArmSectionPoint
        else:
            A = (R ** 2 - S ** 2 + d ** 2) / (2 * d)
            h = math.sqrt(R ** 2 - A ** 2)
            
            x2 = x + A * (a - x) / d
            y2 = y + A * (b - y) / d
            x3 = round(x2 - h * (b - y) / d, 4)
            y3 = round(y2 + h * (a - x) / d, 4)
            x4 = round(x2 + h * (b - y) / d, 4)
            y4 = round(y2 - h * (a - x) / d, 4)
            
            c1 = [x3, y3]
            c2 = [x4, y4]
            
            self.ArmSectionPoint.Point1 = c1
            self.ArmSectionPoint.Point2 = c2
            
            
            return self.ArmSectionPoint

    
            
            
        
        
        
        
        
        
        
    
    
        
    def ConnectedStop(self):
        '''
        断开操控连接

        Returns
        -------
        None.

        '''
        if self.clientID != -1:
        
            # Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
            sim.simxGetPingTime(self.clientID)
    
            sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot)
    
            # Now close the connection to CoppeliaSim:
            sim.simxFinish(self.clientID)
            
            print("Connecte Stoped!!")
            
        else:
            print("Pleace check clientID !")
        
        
            
  

         
            
def main():
    '''
    简易机械臂控制测试

    '''
    MyArmClass = MyArmBasicClass()
    ret = MyArmClass.ConnectedStart()
    
    #position = [()]
    
    if ret == 0: #连接成功
        MyArmClass.GetArmHandle()
        MyArmClass.GetShapeBasePosition()#获取底座坐标
        position1 = MyArmClass.GetJointAngle("ArmJoint1")  #获取三个关节的角度/位置
        position2 = MyArmClass.GetJointAngle("ArmJoint2")
        position1 = MyArmClass.GetJointAngle("BaseJoint")
        
    
        movedir = 0
        movecount = 0
        movestep = 5    #步进量，可用于调速
        moveangle = 90 #目标角度
        
        #让机械臂循环流畅动起来
        
      
        while(True):
            if movedir == 0:
               movecount = movecount-movestep
               if movecount < -moveangle*movestep:
                   movedir = 1
                   
            elif movedir == 1:
                movecount = movecount+movestep
                if movecount > moveangle*movestep:
                    movedir = 0
            MyArmClass.SetJointAngle("ArmJoint1",movecount/10)
            MyArmClass.SetJointAngle("ArmJoint2",movecount/10)
            MyArmClass.SetJointAngle("ArmJoint3",movecount/10)
            MyArmClass.SetJointAngle("BaseJoint",movecount/5000) #可以看到机械臂在上下运动
            time.sleep(1)
        
       
        '''
        while(True):
            movecount= movecount+1
            if movecount%2 == 0:
                goalpoint = (-20,30)
            else:
                goalpoint = (0,30)
            x,y = MyArmClass.CalPointOfSection(0,0,30,goalpoint[0],goalpoint[1],20)
            
            angle1 = MyArmClass.CalAbgleOfPoint(0,0,x,y)
            angle2 = MyArmClass.CalAbgleOfPoint(x,y,goalpoint[0],goalpoint[1])
            print("Angle:",angle1,angle2)
            MyArmClass.SetJointAngle("ArmJoint1",angle1)
            MyArmClass.SetJointAngle("ArmJoint2",angle2+angle1)
            
            time.sleep(1)
        
       
        '''
        

        
        MyArmClass.ConnectedStop() #若是需要断掉，请主动调用
    else:
        print("Connected Error!! Pleace check and retry!!")
        print("需要先在  coppeliasim 软件中开启仿真，再开始本python才能正确远程连接！！")
                      
            
if __name__ == '__main__':
     #multiprocessing.freeze_support()
    main()
          


    